#include <stdio.h>
#include <sys/time.h>
#include <unistd.h>
#include <signal.h>

#include "ethercattype.h"
#include "nicdrv.h"
#include "ethercatbase.h"
#include "ethercatmain.h"
#include "ethercatdc.h"
#include "ethercatcoe.h"
#include "ethercatfoe.h"
#include "ethercatconfig.h"
#include "ethercatprint.h"
#include "math.h"
#include <alchemy/task.h> 
#include <alchemy/timer.h> 

#define TASK_PRIO 99 /* Highest RT priority */ 
#define TASK_MODE 0 /* No flags */ 
#define TASK_STKSZ 0/* Stack size (use default one) */ 
#define TASK_PERIOD  1000000  /* 100 usc period */
#define NSEC_PER_SEC 1000000000
 RT_TASK task_desc; 

char IOmap[4096];
uint16 control;

uint16 control_world;
uint16 state;
int32 curr_position;
int32 position=30000;
int32 position2;
int add_position;
int i=0;
RTIME tt=0;
int64 toff,gl_delta;
// 使从站进入op状态
void slavetop(int i)
{
        ec_send_processdata();
	ec_receive_processdata(EC_TIMEOUTRET);
	ec_slave[i].state = EC_STATE_OPERATIONAL;
	ec_writestate(0);
        ec_statecheck(1, EC_STATE_OPERATIONAL,  EC_TIMEOUTSTATE);
}
void position_to_add(int32 position)
{ 	
	ec_slave[1].outputs[0x002] = position;
	ec_slave[1].outputs[0x003] = (position>>8)&0xff;
	ec_slave[1].outputs[0x004] = (position>>16)&0xff;
	ec_slave[1].outputs[0x005] = (position>>24)&0xff;
	
}

void ec_sync(int64 reftime, int64 cycletime , int64 *offsettime)
{
   static int64 integral = 0;
  
   int64 delta;
   
   /* set linux sync point 50us later than DC sync, just as example */
   delta = (reftime-500000) % cycletime;
   if(delta>(cycletime / 2)) 
   { delta= delta - cycletime; }
  *offsettime = -(delta /20) - (integral /10000);
   integral=integral+delta;
   //d=d-delta; 
   //rt_printf("%lld\n",delta); 
} 
void add_timespec(struct timespec *ts, int64 addtime)
{
   int64 sec, nsec;
   
   nsec = addtime % NSEC_PER_SEC;
   sec = (addtime - nsec) / NSEC_PER_SEC;
   ts->tv_sec += sec;
   ts->tv_nsec += nsec;
   if ( ts->tv_nsec > NSEC_PER_SEC ) 
   { 
      nsec = ts->tv_nsec % NSEC_PER_SEC;
      ts->tv_sec += (ts->tv_nsec - nsec) / NSEC_PER_SEC;
      ts->tv_nsec = nsec;
   }   
}   
int SDOSetting(void)
{
	int retval;
	uint8  u8val;
	uint16 u16val;
	u8val = 0;
	retval += ec_SDOwrite(1, 0x1c12, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);
        u16val = 0x1702;
	retval += ec_SDOwrite(1, 0x1c12, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTRXM);
        
	u8val = 1;
	retval += ec_SDOwrite(1, 0x1c12, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);
	
	u8val = 0;
	retval += ec_SDOwrite(1, 0x1c12, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);
	u16val = 0x1B02;
	retval += ec_SDOwrite(1, 0x1c13, 0x01, FALSE, sizeof(u16val), &u16val, EC_TIMEOUTRXM);
	u8val = 1;
	retval += ec_SDOwrite(1, 0x1c12, 0x00, FALSE, sizeof(u8val), &u8val, EC_TIMEOUTRXM);
        
	printf("AEP slave %d set, retval = %d\n", 1, retval);
   return 1;
}

void simpletest(void *arg)
{        float t=0;
        char* ifname="rteth0";
	if(ec_init(ifname))
	{
	   rt_printf("start ethernet at %s\n",ifname);
	   if ( ec_config_init(FALSE)> 0 )
	   {            
			 rt_printf("found %d slave on the bus\n",ec_slavecount);
                         ec_slave[1].PO2SOconfig = SDOSetting;
                           
                  
                         ec_config_map(&IOmap);
                         ec_configdc();
                         ec_dcsync0(1, TRUE,1000000,00000); 
                        
                                             
			for(i=0;i<ec_slavecount;i++)
			{      
                         
                              slavetop(i); 
                     
                         rt_printf("slave%d to op\n", i);
                         
		}
                       
			     
	      if(ec_slave[0].state == EC_STATE_OPERATIONAL)
             {          
                        
                      
                      
                         struct timespec   ts;
                         clock_gettime(CLOCK_MONOTONIC, &ts);
                         toff = 0;                  
                while(1)
		        {  
                                   
                         add_timespec(&ts, TASK_PERIOD + toff);
                         clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &ts,NULL);
                             ec_send_processdata();
		             ec_receive_processdata(EC_TIMEOUTRET);
                              static int32 step=0;
                              step++;
                              ec_slave[1].outputs[0x000c] = 0x08;
                              if(step>500)
                               {
                                      ec_slave[1].outputs[0x0000] = 0x06;
                                }
                              if(step>1000)
                               {
                                   
                                   ec_slave[1].outputs[0x0000] = 0x07;  
                                }     
                               if(step>1500)
                                 {
                                        ec_slave[1].outputs[0x0000] = 0x0f;
                                        
                                        
                                    
                                         
                                          
                                          
                                 }  
                                 
                                if(step>2000)
                                          {
                                             
                                          position=(int32)(100000000*sin(t));
                                           t=t+0.0001;   
                                          position_to_add( position);
                                          
                                           }
                                 
                                       
                               
                                      ec_sync(ec_DCtime,1000000,&toff);
                               
                             
                              
						
					  uint32 feedb_position;
            int size_position=sizeof(feedb_position);				                     
             ec_SDOread(1,0x607a,0x00,FALSE,&size_position,&feedb_position,EC_TIMEOUTRXM);
			
		       rt_printf("%d\n",feedb_position );                  
			                         
		      }
	           }
			
           }
	
       }
 
		

}

int creat_task(void)
{
                  
              int err; 
            
             
             /* Create a real-time task */ 
            int err_create =rt_task_create(&task_desc,"task_desc",TASK_STKSZ,TASK_PRIO,TASK_MODE); 
              if (err_create!=0)
			  {
				  printf("创建进程错误");
			  } 
             /* If successfully created, start thetask. */ 
           int err_start=rt_task_start(&task_desc,&simpletest,NULL); 
		   if (err_start!=0)
		   {
			  printf("开启任务失败");
		   }
		   if((err_start||err_create)==0)
		   {
              return 0;
		   }
		   else
		   {
			   return -1;
		   }
	 
	      
           
}
int main(int argc, char *argv[])
{
       
	printf("SOEM (Simple Open EtherCAT Master)\nSimple test\n");
	
    
	if (argc > 1)
	{      
               int error=creat_task();
                    if(error<0)
		{
			printf("创建xenomai建成错误");
		}
		
		
               
		
		   
	}
	else
	{
		printf("Usage: simple_test ifname1\nifname = eth0 for example\n");
	}   
               pause();
	printf("End program\n");
	return 0;
}


